#include"globals.h"

string protocol_en(UCHAR CmdID, string data)
{
	PROTOCOL calc_data;
	calc_data.FrameHeader.SOF = 0x21;
	calc_data.FrameHeader.Datalength = data.length();
	calc_data.FrameHeader.CRC = calc_data.FrameHeader.SOF ^ data.length();
	calc_data.CmdID = CmdID;
	calc_data.FrameTail = crccheck(data);
	char* result = new char[ 5 + data.length()];
	result[5 + data.length()] =  0 ;
	result[0] = calc_data.FrameHeader.SOF;
	result[1] = calc_data.FrameHeader.Datalength;
	result[2] = calc_data.FrameHeader.CRC;
	result[3] = calc_data.CmdID;
	result[4] = calc_data.FrameTail+0x20;
	for (UCHAR i = 0; i < data.length(); i++)
	{
		result[i + 5] = data[i];
	}
	string sresult;
	sresult = result;
	return sresult;
}


PROTOCOL protocol_de(string data)
{
	PROTOCOL calc_data;
	if (datacheck(data))
	{
		calc_data.FrameHeader.SOF=data[0];
		calc_data.FrameHeader.Datalength=data[1];
		calc_data.FrameHeader.CRC=data[2];
		calc_data.CmdID=data[3];
		calc_data.DATA.data = new char[data.length()-5];
		calc_data.DATA.data[data.length() - 5] = { 0 };
		for (UCHAR i = 0; i < data.length()-5; i++)
		{
			calc_data.DATA.data[i] = data[5+i];
		}
		calc_data.FrameTail = data[data.length()-1];
		return calc_data;
	}
	else
	{
		calc_data.FrameHeader.SOF = 'E';
		calc_data.FrameHeader.Datalength = 'E';
		calc_data.FrameHeader.CRC = 'E';
		calc_data.CmdID = 'E';
		calc_data.DATA.data = new char[6];
		calc_data.DATA.data = "ERROR!";
		calc_data.FrameTail = 'E';
		return calc_data;
	}
	
}

UCHAR crccheck(string data)
{
	UCHAR x=0;
	for (unsigned int i = 0; i<data.length(); i++)
		x ^= data[i];
	return x;
}

bool datacheck(string data)
{
	
	if (0x21 == data[0] && data[2] == data[0] ^ (data[1]))
	{
		return true;
	}
	else
	{
		return false;
	}
	
}
